/*
 * Controller.cpp
 *
 *      Author: Ben, Philippe, Simon en Tom
 */


#include "Robot.h"

int main()
{
	Robot r;
	//init : zie later
	int arc;
	int dis;
	//bool distance_ok = false;
	//bool arc_ok=false;
	//p = getPacket()
	//r.Robot::UpdateRobot(p);
	//r.Robot::UpdateTarget(p);


	/*TEST CASE*/
	Info i;
	i.robx = 600;
	i.roby = 200;
	i.robhoek = 40;
	i.doelx = 100;
	i.doely = 400;
	i.garx = 0;
	i.gary = 0;

	Udp_header h;
	Udp_pakket d;
	d.header = h;
	d.groep_a = 0;
	d.info_a = i;
	d.groep_b = 1;
	d.info_b = i;
	Pakket p;
	p.nr = 0;
	p.d = d;

	r.UpdateRobot(p);
	r.UpdateTarget(p);

	arc = r.CalcTurn();
	cout << arc << endl;
	dis = r.CalcDistance();
	cout << dis << endl;


	/*
	while(!distance_ok)
	{
		while(!arc_ok)
		{
			//p = getPacket()
			//r.Robot::UpdateRobot(p);

			arc = r.Robot::CalcTurn();
			cout << arc << endl;
			if(arc==0)
			{
				arc_ok = true;
				break;
			}
			r.Robot::Turn(arc);
		}

		distance = r.Robot::CalcDistance();
		if(distance == 0)
		{
			distance_ok = true;
			break;
		}
		r.Robot::Drive(distance);
		arc_ok = false;
	}
	*/
	//Car in position

	//grijpen
}
